#coding=utf-8
#author :sh
import multiprocessing
from threading import Timer
import numpy as np
import cv2
import RPi.GPIO as GPIO
import time
import serial


i = 0
time_interval = 1.0
lower_blue = np.array([0,0,0])
upper_blue = np.array([170,255,46])
cap = cv2.VideoCapture(0)

area_max = 19000
area_min = 6000


pyserial_test=serial.Serial("/dev/ttyS0",115200)

DATA = "123"
A = 000045   ##帧头##
B = 0        ##帧头##
C = 0        ##帧头##
D = 000054   ##帧头##
###########################计算帧率函数############################################
def frame_rate():
	global i
	x = i
	print x
	i = 0
	t = Timer(time_interval,frame_rate)
	t.start()

#####################################################################################


###########################串口发送函数##############################################
def uart_send():
	global DATA
	pyserial_test.write(DATA)
	
#####################################################################################


###########################主图像处理函数############################################
def main():
	uart_send_data = multiprocessing.Process(name = "uart_send_data",
			target = uart_send,
			args = ())
	while(True):
		global i
		i = i + 1
		X = []
		Y = []
		ret, frame = cap.read()
		gray   = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
		ret,thresh=cv2.threshold(gray,160,255,cv2.THRESH_BINARY)
		contours, hierarchy = cv2.findContours(
											thresh,
											cv2.RETR_TREE,
											cv2.CHAIN_APPROX_NONE)
		#寻找轮廓
		F = []
		for j in range(len(contours)):
			area = cv2.contourArea(contours[j])
			if (area > area_min)and(area < area_max):
				# print "Area",cv2.contourArea(contours[i])
				F.append(contours[j])
###############寻找重心############################################################
		for j in range(len(F)):
			M = cv2.moments(F[j])
			X.append( (M['m10'])/(M['m00']))
			Y.append( (M['m01'])/(M['m00']))
		XC = 0
		YC = 0
		for j in range(len(F)):
			XC = X[j]
			YC = Y[j]
		# print 'XC',XC
		# print 'YC',YC
		B = int(XC)
		C = int(YC)
#####################串口发送处理函数###############################################
		# cv2.drawContours(gray, F, -1, (0,255,0), 3)
		# cv2.imshow("gray",gray )
		str_b = str(B)
		str_c = str(C)
		while (len(str_b) < 6):
			str_b = '\0' + str_b
		while (len(str_c) < 6):
			str_c = '\0' + str_c
		DATA = str(A) + str_b + str_c + str(D) +'\n'
		# time.sleep(1)
		pyserial_test.write(DATA)
		#############串口接收#####################################################
		count=pyserial_test.inWaiting()
		if count!=0:
			recv = pyserial_test.read(count)+"\n"
			# print recv
			pyserial_test.write(recv)
			pyserial_test.flushInput()
		if(recv = "go"):
			print "go"
		if cv2.waitKey(1) & 0xFF == ord('q'):
			break
	cap.release()
	cv2.destroyAllWindows()
######################################################################################


###########函数入口###################################################################
if __name__=='__main__':
	pyserial_test.write(str(A) + "pi is ready!" + str(D) + "\n")
	time.sleep(5)
	t = Timer(time_interval,frame_rate)
	t.start()
	try:
		main()
	except KeyboardInterrupt:
		if pyserial_test!=None:
			pyserial_test.close()



